#include "ogl/math/Transform.h"

using namespace Ogl::Math;
using namespace Ogl;

void Transform::Decompose(const glm::mat4 &source)
{
	CreateFrom(source);
};

void Transform::CreateFrom(const glm::mat4 &local)
{

	glm::vec3 skew;
	glm::vec4 perspective;
	glm::quat rot;

	glm::decompose(local, scale, rot, position, skew, perspective);

	rotation = glm::degrees(glm::eulerAngles(rot));

}

Transform::Transform()
{
}

glm::vec3 Transform::GetRight() const
{
	glm::mat4 rMat = GetRotationMatrix();
	return rMat[0];
}

glm::vec3 Transform::GetForward() const
{
	glm::mat4 rMat = GetRotationMatrix();
	return rMat[2];
}

glm::vec3 Transform::GetUp() const
{
	glm::mat4 rMat = GetRotationMatrix();
	return rMat[1];
}

glm::mat4 Transform::GetViewMatrix() const
{
	return glm::lookAt(position, position + GetForward(), GetUp());
}

glm::mat4 Ogl::Math::Transform::GetViewMatrixNoTranslation() const
{
	return glm::mat4(glm::mat3(GetViewMatrix()));
	// remove translation from the view matrix

}
glm::mat4 Transform::GetWorld() const
{

	glm::mat4 model(1.0f);

	model = glm::translate(model, position);
	model = glm::rotate(model, glm::radians(rotation.z), glm::vec3(0.0f, 0.0f, 1.0f));
	model = glm::rotate(model, glm::radians(rotation.y), glm::vec3(0.0f, 1.0f, 0.0f));
	model = glm::rotate(model, glm::radians(rotation.x), glm::vec3(1.0f, 0.0f, 0.0f));

	model = glm::scale(model, scale);

	return model;
}

glm::mat4 Math::Transform::GetPosRotMatrix() const
{
	glm::mat4 model(1.0f);
	model = glm::translate(model, position);
	model = glm::rotate(model, glm::radians(rotation.z), glm::vec3(0.0f, 0.0f, 1.0f));
	model = glm::rotate(model, glm::radians(rotation.y), glm::vec3(0.0f, 1.0f, 0.0f));
	model = glm::rotate(model, glm::radians(rotation.x), glm::vec3(1.0f, 0.0f, 0.0f));
	return model;
}

void Transform::RotateX(float d)
{

	rotation.x += d;
}

void Transform::RotateY(float d)
{
	rotation.y += d;
}

void Transform::RotateZ(float d)
{
	rotation.z += d;
}

void Transform::SetRotation(const glm::quat &q)
{
	glm::vec3 rot = glm::eulerAngles(q);
	rotation = rot;
	this->rotation *= 180.0f / glm::pi<float>();
}
void Transform::Scale(float d)
{
	this->scale *= d;
}

void Transform::Scale(glm::vec3 o, float d)
{
	glm::vec3 delta = o - position;
	float len = glm::length(delta);
	glm::vec3 frd = glm::normalize(delta);

	this->position = o;
	this->scale *= d;
	this->position += delta * d * frd;
}

void Transform::BackAt(glm::vec3 target, glm::vec3 worldUp)
{
	if (target == this->position)
	{
		return;
	}
	glm::mat4 view = glm::lookAtRH(position, target, worldUp);
	rotation = GetEulerAnglesFromRotationMatrix(glm::inverse(view));
}

void Transform::LookAt(glm::vec3 target, glm::vec3 worldUp)
{
	BackAt(2.0f * position - target, worldUp);
}

glm::mat4 Transform::GetRotationMatrix() const
{
	glm::mat4 model(1.0f);
	model = glm::rotate(model, glm::radians(rotation.z), glm::vec3(0.0f, 0.0f, 1.0f));
	model = glm::rotate(model, glm::radians(rotation.y), glm::vec3(0.0f, 1.0f, 0.0f));
	model = glm::rotate(model, glm::radians(rotation.x), glm::vec3(1.0f, 0.0f, 0.0f));
	return model;
}

void Transform::Move(float dist, glm::vec3 f)
{
	position += dist * glm::normalize(f);
}

void Transform::Translate(float dist)
{
	this->position += this->GetForward() * dist;
}

void Transform::MoveRight(float dist)
{
	this->position += this->GetRight() * dist;
}

glm::vec3 Transform::GetPosXZ() const
{
	glm::vec3 forward = GetForward();

	float f = forward.y;
	float p = position.y;

	if (f == 0.0f)
	{
		return glm::vec3();
	}

	if (p * f < 0.0f)
	{
		float len = -p / f;
		return len * forward + position;
	}
	return glm::vec3(0.f);
}

glm::vec3 Transform::GetPosYZ() const
{
	glm::vec3 forward = GetForward();

	float f = forward.x;
	float p = position.x;

	if (p * f <= 0.0f)
	{
		float len = -p / f;
		return len * forward + position;
	}
	return glm::vec3(0.0f);
}

glm::vec3 Transform::GetPosXY() const
{
	glm::vec3 forward = GetForward();

	float f = forward.z;
	float p = position.z;

	if (p * f <= 0.0f)
	{
		float len = -p / f;
		return len * forward + position;
	}
	return glm::vec3();
}

float Transform::GetToXZSq()
{
	glm::vec3 d2 = position - GetPosYZ();
	return glm::length(d2);
}

void Transform::Rotate(glm::vec3 axis)
{
	RotateZ(axis.z);
	RotateX(axis.y);
	RotateY(axis.z);
}

Transform::Transform(const glm::vec3 &pos, const glm::quat &rot, const glm::vec3 &scale)
{
	this->position = pos;
	SetRotation(rot);
	this->scale = scale;
}

glm::quat Transform::GetRoatationQuat() const
{
	return glm::quat(rotation / 180.0f * glm::pi<float>());
}

void Transform::SetPosition(const glm::vec3 &pos)
{
	this->position = pos;
}

void Transform::SetScale(const glm::vec3 &s)
{
	this->scale = s;
}

glm::vec3 Transform::Yup()
{
	return glm::vec3(0.0f, 1.0f, 0.0f);
}
glm::vec3 Transform::Xup()
{
	return glm::vec3(1.0f, .0f, 0.0f);
}

glm::vec3 Transform::Zup()
{
	return glm::vec3(0.0f, .0f, 1.0f);
}

Transform Transform::Slerp(const Transform &t1, const Transform &t2, float axis)
{
	return Transform();
}

glm::vec3 Ogl::Math::Transform::GetEulerAnglesFromRotationMatrix(const glm::mat4 &rotationMatrix)
{

	float c = sqrtf(1.0f - rotationMatrix[2][1] * rotationMatrix[2][1]);
	if (glm::isnan(c))
	{
		c = 0.f;
	}
	glm::vec3 rotation;

	rotation.z = glm::atan(rotationMatrix[0][1], rotationMatrix[1][1]);
	rotation.x = glm::atan(-rotationMatrix[2][1], c);
	rotation.y = glm::atan(rotationMatrix[2][0], rotationMatrix[2][2]);

	// theta phi pitch
	// ˳�򲻶�
	return rotation * 180.0f / glm::pi<float>();
}